{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "1362c005",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Sucessfully connect to the robot CAN=can_left\n",
      "Sucessfully make arm can_left enabled\n",
      "Use camera model: L515\n",
      "load camera intrinsics from camera/L515/intrinsics.npz\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/knowin/codes/knowin-robot/robots/piper.py:123: UserWarning: Gimbal lock detected. Setting third angle to zero since it is not possible to uniquely determine all angles.\n",
      "  self.wait_motion_done()\n",
      "/home/knowin/codes/knowin-robot/robots/piper.py:260: UserWarning: Gimbal lock detected. Setting third angle to zero since it is not possible to uniquely determine all angles.\n",
      "  while self.ik.et_error(self.get_joint_status() + 1e-6) > 0.01:  # 1cm tolerance\n"
     ]
    }
   ],
   "source": [
    "import json\n",
    "import os\n",
    "import sys\n",
    "import time\n",
    "\n",
    "import cv2\n",
    "import numpy as np\n",
    "\n",
    "from camera.camera import Camera, CameraBaseTransforamtion\n",
    "from robots.piper import PiperArm\n",
    "\n",
    "can_port = 'can_left'\n",
    "arm = PiperArm(can_port, wait_execution=True)\n",
    "arm.gripper_slope = 1\n",
    "camera = Camera()\n",
    "transform = CameraBaseTransforamtion(f\"camera/{camera.model}/{arm.can}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "00003b58",
   "metadata": {},
   "outputs": [],
   "source": [
    "x, y = 860, 412\n",
    "\n",
    "camera_ood = camera.get_3d_point_from_pixel(x, y)\n",
    "cam2arm_cood = transform(camera.get_3d_point_from_pixel(x, y))\n",
    "arm.move_to(cam2arm_cood)\n",
    "time.sleep(2)\n",
    "arm.step_ik()\n",
    "arm_cood= arm.ik.get_effector_xeuler().tolist()[:3]\n",
    "\n",
    "# print()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "c4de7d07",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.6429992630905597, -0.40365713659778274, 0.053967372752661655]\n"
     ]
    }
   ],
   "source": [
    "print(arm_cood)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "f2b58854",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.go_zero()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "502de262",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[602.72851562,   0.        , 328.12030029],\n",
       "       [  0.        , 602.77764893, 240.51695251],\n",
       "       [  0.        ,   0.        ,   1.        ]])"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "camera.camera_matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "5d592a7e",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 0.1273181 , -0.43172154, -0.00046047,  0.00091564,  0.38736609])"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "camera.dist_coeffs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "97ed8a15",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-0.05694025  0.26717794  0.74100006]\n",
      "[-0.0796426   0.2511518   0.76325005]\n"
     ]
    }
   ],
   "source": [
    "cood1 = camera.get_3d_point_from_pixel(591, 702)\n",
    "cood2 = camera.get_3d_point_from_pixel(566, 673)\n",
    "\n",
    "print(cood1)\n",
    "print(cood2)\n",
    "\n",
    "piper_cood1 = "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "8c628162",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-1"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "image = camera.capture_current_frame()\n",
    "cv2.imshow('image',  image)\n",
    "cv2.waitKey(0)\n",
    "\n",
    "# 591, 702\n",
    "# 566, 673"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a249b843",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "mujoco",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.18"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
